#include "PodMath.h"

PodMath::PodMath() {}
PodMath::~PodMath() {}


//Vector Conversions
PodMath::pVec3* PodMath::OgreVectorToPVec(Ogre::Vector3 vec)
{
	PodMath::pVec3 *newvec = new PodMath::pVec3();
	newvec->initialize(vec.x, vec.y, vec.z);
	return newvec;
}

PodMath::pVec3* PodMath::IrrKlangVectorToPvec(irrklang::vec3d<float> vec)
{
	PodMath::pVec3 *newvec = new PodMath::pVec3();
	newvec->initialize(vec.X, vec.Y, vec.Z);
	return newvec;
}

Ogre::Vector3 PodMath::PVecToOgreVector(PodMath::pVec3* vec)
{
	return Ogre::Vector3(vec->x, vec->y, vec->z);
}

Ogre::Vector3 PodMath::IrrKlangVectorToOgreVector(irrklang::vec3d<float> vec)
{
	return Ogre::Vector3(vec.X, vec.Y, vec.Z);
}

irrklang::vec3d<float> PodMath::PVecToIrrKlangVector(PodMath::pVec3* vec)
{
	irrklang::vec3d<float> newvec;
	newvec.X = vec->x;
	newvec.Y = vec->y;
	newvec.Z = vec->z;
	return newvec;
}

irrklang::vec3d<float> PodMath::OgreVectorToIrrKlangVector(Ogre::Vector3 vec)
{
	irrklang::vec3d<float> newvec;
	newvec.X = vec.x;
	newvec.Y = vec.y;
	newvec.Z = vec.z;
	return newvec;
}
//End Vector Conversions


	

//Fast Math

/* //NEED TO FIX!!! - Zach
float PodMath::fast_rand(void)
{
	__asm
	{
		movq mm0, q
		pshufw mm1, mm0, 0x1E
		paddd mm0, mm1
		movq q, mm0
		emms
	}
}
*/

float PodMath::fast_sqrt(float x)
{
	__asm 
	{
        fld x
        fsqrt
	}
}

float PodMath::fast_invsqrt(float x)
{
	__asm
	{
		fld x
		fmul HALF_F
		mov eax, x
		shr eax, 0x1
		neg eax
		add eax, 0x5F400000
		mov x, eax
		fmul x
		fmul x
		fsubr ONE_HALF_F
		fmul x
	}
}


__declspec(naked) float PodMath::fast_invsqrtM$(float fValue) // still needs to be fixed -- Zach
{
	__asm
	{
		mov eax, 0be6eb508h
		mov dword ptr[esp-12], 03fc00000h
		sub eax, dword ptr[esp+4]
		sub dword ptr[esp+4], 800000h
		shr eax, 1
		mov dword ptr[esp-8], eax
		fld dword ptr[esp-8]
		fmul st, st
		fld dword ptr[esp-8]
		fxch st(1)
		fmul dword ptr[esp+4]
		fld dword ptr[esp-12]
		fld st(0)
		fsub st, st(2)
		fld st(1)
		fxch st(1)
		fmul st(3), st
		fmul st(3), st
		fmulp st(4), st
		fsub st, st(2)
		fmul st(2), st
		fmul st(3), st
		fmulp st(2), st
		fxch st(1)
		fsubp st(1), st
		fmulp st(1), st
		ret 4
	}
}


float PodMath::fast_ln(float x)
{
	int log_2 = 0;
	__asm
	{
		mov eax, x
		sar eax, 0x17
		and eax, 0xFF
		sub eax, 0x80
		mov log_2, eax
		mov ebx, x
		and ebx, 0x807FFFFF
		add ebx, 0x3F800000
		mov x, ebx
		fld x
		fmul NEG_ONE_THREE_F
		fadd TWO_F
		fmul x
		fadd NEG_TWO_THREE_F
		fild log_2
		fadd
		fmul LN2
	}
}

float PodMath::fast_sin(float x)
{
	__asm
	{
		fld x
		fsin
	}
}

float PodMath::fast_cos(float x)
{
	__asm
	{
		fld x
		fcos
	}
}

float PodMath::fast_tan(float x)
{
	__asm
	{
		fld x
		fsin
		fld x
		fcos
		fdiv
	}
}

float PodMath::fast_csc(float x)
{
	__asm
	{
		fld1
		fld x
		fsin
		fdiv
	}
}

float PodMath::fast_sec(float x)
{
	__asm
	{
		fld1
		fld x
		fcos
		fdiv
	}
}

float PodMath::fast_cot(float x)
{
	__asm
	{
		fld1
		fld x
		fsin
		fld x
		fcos
		fdiv
		fdiv
	}
}

float PodMath::fast_arcsin(float x)
{
	__asm
	{
		fld x
		fld x
		fmul x
		fsubr ONE_F
		fsqrt
		fdiv
		fld1
		fpatan
	}
}

float PodMath::fast_arccos(float x)
{
	__asm 
	{
        fld x
        fld x 
        fmul x 
        fsubr ONE_F
        fsqrt
        fchs
        fdiv
        fld1
        fpatan
        fadd PI_H 
    } 
}

float PodMath::fast_arctan(float x)
{
	__asm
	{
		fld x
		fld1
		fpatan
	}
}
